/*
 * Assignment06.c
 *
 * Created: 4/27/2015 6:03:35 PM
 *  Author: Jeevake Attapattu
 */ 

#define F_CPU 8000000UL
#include <avr/io.h>
#include <util/delay.h>
#include <stdio.h>
#include <util/twi.h>
#include "i2cmaster.h"

#include <pololu/orangutan.h>


#define MPU6050  0xD0     // (0x68 << 1) I2C slave address

unsigned char ret;            // return value

char outs[50];

//********************************//



void usart_init (void)
{
	//synchronous usart, transmit 8-bit data
	UCSR0C = ((1<<UCSZ01)|(1<<UCSZ00)|(1<<UMSEL00));
	//9600 Baud Rate from 8MHz clock
	UBRR0H = 0x01;
	UBRR0L = 0xA0;
	UCSR0B = (1<<TXEN0);	//enable transmitter
}
void USART_tx_string (char *data)
{
	while((*data!='\0')){
		while(!(UCSR0A&(1<<UDRE0)));			//wait until transmit register is empty
		UDR0 = *data;
		data++;
	}
}





void MPU6050_writereg(uint8_t reg, uint8_t val)
{
	i2c_start(MPU6050+I2C_WRITE);
	i2c_write(reg);  // go to register e.g. 106 user control
	i2c_write(val);  // set value e.g. to 0100 0000 FIFO enable
	i2c_stop();        // set stop condition = release bus
}


uint16_t MPU6050_readreg(uint8_t reg)
{
	
	i2c_start_wait(MPU6050+I2C_WRITE); // set device address and write mode
	i2c_write(reg);                                  // ACCEL_XOUT
	i2c_rep_start(MPU6050+I2C_READ);    // set device address and read mode
	int raw = i2c_readAck();                    // read one intermediate byte
	raw = (raw<<8) | i2c_readNak();        // read last byte
	i2c_stop();

	return raw;
}

uint16_t MPU6050_signed_readreg(uint8_t reg)
{
	
	i2c_start_wait(MPU6050+I2C_WRITE); // set device address and write mode
	i2c_write(reg);                                  // ACCEL_XOUT
	i2c_rep_start(MPU6050+I2C_READ);    // set device address and read mode
	int raw = i2c_readAck();                    // read one intermediate byte
	if(raw>=128)	//check for negative values by MSB value
		raw = raw*(-1);
	raw = (raw*8) + i2c_readNak();        // read last byte
	i2c_stop();

	return raw;
}

void Init_MPU6050()
{

	i2c_init();     // init I2C interface

	_delay_ms(200);  // Wait for 200 ms.

	ret = i2c_start(MPU6050+I2C_WRITE);       // set device address and write mode
	if ( ret )
	{
		snprintf(outs,sizeof(outs),"failed to issue start condition \n\r");
		USART_tx_string(outs);
		i2c_stop();
	}
	else
	{
		/* issuing start condition ok, device accessible */
		MPU6050_writereg(0x6B, 0x00); // reg 107 set value to 0000 0000 and wake up sensor
		MPU6050_writereg(0x19, 0x07); // reg 25 sample rate divider set value to 0000 1000 for 1000 Hz
		MPU6050_writereg(0x1C, 0x18); // reg 28 acceleration configuration set value to 0001 1000 for 16g
		MPU6050_writereg(0x23, 0xF8); // reg 35 FIFO enable set value to 1111 1000 for all sensors not slave
		MPU6050_writereg(0x37, 0x10); // reg 55 interrupt configuration set value to 0001 0000 for logic level high and read clear
		MPU6050_writereg(0x38, 0x01); // reg 56 interrupt enable set value to 0000 0001 data ready creates interrupt
		MPU6050_writereg(0x6A, 0x40); // reg 106 user control set value to 0100 0000 FIFO enable
		snprintf(outs,sizeof(outs),"done start \n\r");
		USART_tx_string(outs);
	}
	i2c_stop();
}



int main(){
	
	DDRD = 0xF0;
	DDRC = 0x00;
	int xa, ya, za;
	usart_init();
	snprintf(outs,sizeof(outs),"initialized uart \n\r");
	USART_tx_string(outs);
	Init_MPU6050();    // Sensor init
	_delay_ms(200);     // Wait for 200 ms.
	snprintf(outs,sizeof(outs),"6050 initialized \n\r");
	USART_tx_string(outs);

	//Start infinite loop
	while(1){

		xa = MPU6050_signed_readreg(0x3B);   // read raw X acceleration from fifo
		snprintf(outs,sizeof(outs),"XA %d, ",xa+'0');
		USART_tx_string(outs);
		ya = MPU6050_signed_readreg(0x3D);   // read raw Y acceleration from fifo
		snprintf(outs,sizeof(outs),"YA %d, ",ya+'0');
		USART_tx_string(outs);
		za = MPU6050_signed_readreg(0x3F);   // read raw Z acceleration from fifo
		snprintf(outs,sizeof(outs),"ZA %d \n\r",za+'0');
		USART_tx_string(outs);
		_delay_ms(500);
		
	}
	return 0;
}